#include "Util.h"
#include "asNode.hpp"


bool asNodePathSort(PathXY n1, PathXY n2)
{
	return false;
}

void backtrace(asNode * node,Path &path)
{
	PathXY _t;
	_t.push_back(node->x);
	_t.push_back(node->y);
	
	path.push_back(_t);

	while (node->parent) {
		node = node->parent;

		PathXY __t;
		__t.push_back(node->x);
		__t.push_back(node->y);

		path.push_back(__t);
	}
};

Path biBacktrace(asNode* nodeA,  asNode* nodeB)
{
	Path ret;
	Path pathA;
	backtrace(nodeA,pathA);
	Path pathB;
	backtrace(nodeB,pathB);

	
	std::sort(pathB.begin(),pathB.end(),asNodePathSort);

	ret.reserve ( pathA.size() + pathB.size() );
	copy ( pathA.begin(), pathA.end(), ret.begin() );
	copy ( pathB.begin(), pathB.end(), ret.begin() + pathA.size() );

	return ret;
	//pathA.insert(pathA.end(),pathB.begin(),pathB.end());
}

int pathLength(Path path) {
	int i, sum = 0, a, b, dx, dy;
	
	for (i = 1; i < path.size(); ++i) {
		PathXY a = path[i - 1];
		PathXY b = path[i];

		dx = a[0] - b[0];
		dy = a[1] - b[1];

		sum += sqrt(dx * dx + dy * dy);
	}

	return sum;
}

Path interpolate(int x0, int y0, int x1, int y1)
{
	Path line;
	int	sx, sy, dx, dy, err, e2;

	dx = abs(x1 - x0);
	dy = abs(y1 - y0);

	sx = (x0 < x1) ? 1 : -1;
	sy = (y0 < y1) ? 1 : -1;

	err = dx - dy;

	while (true) {
		PathXY temp;
		temp.push_back(x0);
		temp.push_back(y0);

		line.push_back(temp);

		if (x0 == x1 && y0 == y1) {
			break;
		}

		e2 = 2 * err;
		if (e2 > -dy) {
			err = err - dy;
			x0 = x0 + sx;
		}
		if (e2 < dx) {
			err = err + dx;
			y0 = y0 + sy;
		}
	}
	return line;
}

Path expandPath(Path path) {

	Path expanded;
	Path interpolated;

	PathXY coord0, coord1;
    int	len = path.size(), interpolatedLen, i, j;

	if (len < 2) {
		return expanded;
	}

	for (i = 0; i < len - 1; ++i) {
		coord0 = path[i];
		coord1 = path[i + 1];

		interpolated = interpolate(coord0[0], coord0[1], coord1[0], coord1[1]);
		interpolatedLen = interpolated.size();

		for (j = 0; j < interpolatedLen - 1; ++j) {
			expanded.push_back(interpolated[j]);
		}
	}

	expanded.push_back(path[len - 1]);

	return expanded;
}

Path smoothenPath(Grid *grid, Path &path) {

	int len = path.size(),
		x0 = path.at(0).at(0),        // path start x
		y0 = path.at(0).at(1),        // path start y
		sx, sy,                 // current start coordinate
		ex, ey,                 // current end coordinate
		lx, ly,                 // last valid end coordinate
		i, j;

	int x1 = path.at(len - 1).at(0),  // path end x
		y1 = path.at(len - 1).at(1); // path end y

	bool blocked;
	
	Path newPath , line;
	PathXY coord ,testCoord;

	sx = x0;
	sy = y0;
	lx = path.at(1).at(0);
	ly = path.at(1).at(1);

	PathXY _temp;
	_temp.push_back(sx);
	_temp.push_back(sy);

	newPath.push_back(_temp);

	for (i = 2; i < len; ++i) {

		coord = path.at(i);
		ex = coord.at(0);
		ey = coord.at(1);

		line = interpolate(sx, sy, ex, ey);

		blocked = false;
		for (j = 1; j < line.size(); ++j) {
			testCoord = line.at(j);

			if (!grid->isWalkableAt(testCoord.at(0), testCoord.at(1))) {
				blocked = true;

				PathXY _tempL;
				_tempL.push_back(lx);
				_tempL.push_back(ly);

				newPath.push_back(_tempL);
				sx = lx;
				sy = ly;
				break;
			}
		}
		if (!blocked) {
			lx = ex;
			ly = ey;
		}
	}

	PathXY _t;
	_t.push_back(x1);
	_t.push_back(y1);
	newPath.push_back(_t);


	path.clear();

	return newPath;
}

Path compressPath(Path path) {

	// nothing to compress
	if(path.size() < 3) {
		return path;
	}

	Path compressed;


	int	sx = path[0][0], // start x
		sy = path[0][1], // start y
		px = path[1][0], // second point x
		py = path[1][1], // second point y
		dx = px - sx, // direction between the two points
		dy = py - sy, // direction between the two points
		lx, ly,
		ldx, ldy,
		sq, i;

	// normalize the direction
	sq = sqrt(dx*dx + dy*dy);
	dx /= sq;
	dy /= sq;

	// start the new path
	PathXY _t;
	_t.push_back(sx);
	_t.push_back(sy);

	compressed.push_back(_t);

	for(i = 2; i < path.size(); i++) {

		// store the last point
		lx = px;
		ly = py;

		// store the last direction
		ldx = dx;
		ldy = dy;

		// next point
		px = path[i][0];
		py = path[i][1];

		// next direction
		dx = px - lx;
		dy = py - ly;

		// normalize
		sq = sqrt(dx*dx + dy*dy);
		dx /= sq;
		dy /= sq;

		// if the direction has changed, store the point
		if ( dx != ldx || dy != ldy ) {
			PathXY __t;
			__t.push_back(lx);
			__t.push_back(ly);;
			compressed.push_back(__t);
		}
	}

	// store the last point

	PathXY ___t;
	___t.push_back(px);
	___t.push_back(py);
	compressed.push_back(___t);

	return compressed;
}